#ifndef STEREO_UNDISTORT_HPP
#define STEREO_UNDISTORT_HPP

#include "common_include.hpp"
using namespace cv;

#define undistort_pic_buffer_size 5

class Stereo_Undistort{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        typedef std::shared_ptr<Stereo_Undistort> Ptr;
        Stereo_Undistort();
        ~Stereo_Undistort();

        void Stereo_Undistort_Stop();
        void Stereo_Undistort_Process(unsigned char* g_pRgbBuffer_left,unsigned char* g_pRgbBuffer_right);
    private:
        void Stereo_Undistort_Loop();

        std::thread stereo_undistort_thread_;
        std::mutex data_mutex_;
        
        std::condition_variable pic_process_;
        std::atomic<bool> stereo_undistort_running_;
        

        Mat Left_Raw_K,Right_Raw_K;
        Mat Left_New_K,Right_New_K;
        Mat Left_distCoeff,Right_distCoeff; 
        Mat Rotation_r2l,Translation_r2l;

        int imgWidth,imgHeight;
        Size imageSize,undistort_imageSize;
        double alpha;
        Mat Left_Undistort_Rotation,Right_Undistort_Rotation;
        Mat Q;
        Mat rectifyImageL,rectifyImageR;
        Rect validRoiL,validRoiR;
        Mat rmap[2][2];
        unsigned char* g_pRgbBuffer_left_;
        unsigned char* g_pRgbBuffer_right_;

        Mat* left_undistort_pic_[undistort_pic_buffer_size];
        Mat* right_undistort_pic_[undistort_pic_buffer_size];

        int pic_undistort_index_;
        bool finish_undistort_flag_ = false;
};

#endif